// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include <iostream>
#include "canconfig.h"
#include <chrono>
#include <thread>
#include <iostream>
#include <cmath>
#include <stdio.h>

using namespace std::chrono_literals;
BYTE get_data[2] = {0x01, 0x00};
int encoder_id = 0x0000;
int joint_1data;

// 多线程
int ret;
int m_run0 = 1;
pthread_t threadid;

// 3.自定义节点类；
class Arm_Encoder : public rclcpp::Node
{
public:
    Arm_Encoder() : Node("arm_encoder_node_cpp")
    {
        // 启动发布
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        SendData(joint_encoder, encoder_id, get_data, 2);
        usleep(100000); // 延时100ms。
        std::cout << "已发送" << std::endl;
        // 关节一发布方
        joint1_pub = this->create_publisher<std_msgs::msg::Int32>("joint1_p", 5);
        // 创建接收函数
        ret = pthread_create(&threadid, NULL, &Arm_Encoder::receive_func, this);
    }

    ~Arm_Encoder()
    {
        pthread_join(threadid, NULL); // 等待线程结束
    }

private:
    // 创建发布方
    // 创建关节一位置信息发布
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint1_pub;
    static void *receive_func(void *param)
    {
        Arm_Encoder *encoder = static_cast<Arm_Encoder*>(param);
        int reclen = 0;
        int ind = 1;
        int i, j;  
        float angle_joint1;
        auto joint1_p = std_msgs::msg::Int32();
        while (rclcpp::ok())
        {
            usleep(100000); // 延时0.1s。
            printf("进入了循环\n");
            if ((reclen = VCI_Receive(VCI_USBCAN2, 0, ind, rec, 100, 100)) > 0)
            {
                for (j = 0; j < reclen; j++)
                {
                    // printf("RX ID:0x%08X", rec[j].ID); // ID
                    switch (rec[j].ID)
                    {
                    case 0x0000182:
                    {
                        printf("RX ID:0x%08X", rec[j].ID); // ID
                        printf("\n");
                        for (i = 0; i < rec[j].DataLen; i++)
                        {
                            receiveData[i] = rec[j].Data[i];
                            printf(" data:%02X", rec[j].Data[i]);
                        }
                        uint8_t changeData[3];
                        uint32_t hexNumber = 0;
                        int32_t encoder2_data;
                        changeData[0] = receiveData[2];
                        changeData[1] = receiveData[1];
                        changeData[2] = receiveData[0];
                        printf("\n");

                        // 将data中的三个字节组合成一个32位的十六进制数
                        hexNumber |= changeData[0]; // 将第一个字节赋值给hexNumber的最高8位
                        hexNumber <<= 8;            // 左移8位，为下一个字节腾出空间
                        hexNumber |= changeData[1]; // 将第二个字节赋值给hexNumber的次高8位
                        hexNumber <<= 8;            // 左移8位，为下一个字节腾出空间
                        hexNumber |= changeData[2]; // 将第三个字节赋值给hexNumber的最低8位

                        printf("hexNumber: 0x%06X\n", hexNumber);

                        // 检查符号位
                        if (hexNumber & 0x800000)
                        {
                            // 如果符号位为1，表示负数
                            // 将补码转换为正数的补码表示
                            // uint32_t positiveComplement = ~hexNumber + 1;
                            // // 将得到的绝对值取负
                            // int32_t negativeValue = -((int32_t)positiveComplement);
                            hexNumber |= 0xFF000000; // 扩展符号位到32位
                            encoder2_data = hexNumber;
                            // int32_t negativeValue = (int32_t)hexNumber; // 直接转换为有符号
                            // encoder2_data = negativeValue;
                        }
                        else
                        {
                            encoder2_data = hexNumber;
                            // 如果符号位为0，表示正数
                        }
                        angle_joint1 = ((float)encoder2_data) * 360 / 16384;
                        // 需要四舍五入成整数，然后以Int形式发布
                        joint_1data = round(angle_joint1 * 1.18);
                        // joint_1data = 5;
                        printf("角度是：%d \n", joint_1data);
                        joint1_p.data = joint_1data;
                        encoder->joint1_pub->publish(joint1_p);
                        break;
                    }
                    }
                }
            }
        }
        return nullptr;
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 初始化CAN设备
    Init_Can();
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<Arm_Encoder>());
    // 5.资源释放
    rclcpp::shutdown();
    // 关闭CAN设备
    VCI_CloseDevice(device_type, device_index);
    return 0;
}

// CAN设备初始化函数
void Init_Can(void)
{
    // 初始化CAN设备
    device_type = VCI_USBCAN2; // 设备类型修改
    device_index = 0;          // 设备的索引
    can_index = 1;             // CAN控制器的索引 CAN1为0，CAN2为1

    if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
    {
        std::cout << "无法开启CAN设备" << std::endl;
        return;
    }
    else
    {
        std::cout << "设备开启,如需退出请按“CTRL + C”" << std::endl;
    }

    if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
    {
        std::cout << "获取设备信息失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }
    // 设置CAN初始化配置
    init_config.AccCode = 0X00000000;
    init_config.AccMask = 0xFFFFFFFF;
    init_config.Filter = 2; // 只接受标准帧
    init_config.Mode = 0;
    init_config.Timing0 = 0x01;
    init_config.Timing1 = 0x1c; // 波特率：250kps

    if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
    {
        std::cout << "初始化CAN参数失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }

    // 开始CAN通讯
    if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
    {
        std::cout << "开启CAN通道失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }
}

// 指令发送函数  VCI_CAN_OBJ   帧ID  要发送的帧   帧长度
void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data, int dataLen)
{
    handle_obj.ID = id;
    handle_obj.RemoteFlag = 0;
    handle_obj.ExternFlag = 0;
    handle_obj.DataLen = dataLen;
    for (int i = 0; i < handle_obj.DataLen; i++)
    {
        handle_obj.Data[i] = data[i];
    }
    if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
    {
    }
    else
    {
        std::cout << "初始化错误！" << std::endl;
    }
}